/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package frc2856.robot;

import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.SpeedController;

/**
 *
 * @author Dustin Doss
 */
public class HolonomicDrive extends RobotDrive{
    
    public final int FRONTLEFTSC=0, BACKLEFTSC=1, FRONTRIGHTSC=2, BACKRIGHTSC=3;
    SpeedController[] speedControllers = new SpeedController[4];
    Gyro gyro;
    int gyroOffset;
    boolean useGyro;
    
    public HolonomicDrive(SpeedController sc0, SpeedController sc1, SpeedController sc2, SpeedController sc3, Gyro g){
        super(sc0, sc1, sc2, sc3);
        speedControllers[FRONTLEFTSC] = sc0;
        speedControllers[BACKLEFTSC] = sc1;
        speedControllers[FRONTRIGHTSC] = sc2;
        speedControllers[BACKRIGHTSC] = sc3;
        gyro = g;
	gyroOffset = 0;
	useGyro = false;//CHANGE TO TRUE IF GYRO AND UNCOMMENT THE "DRIVE" FUNCTIONALITY
    }
    
    public void drive(float mag, float dir, float rot) {
	if(useGyro) {
	    super.mecanumDrive_Polar(mag, dir, gyro.getAngle() + gyroOffset+rot);
	} else {
	    super.mecanumDrive_Polar(mag, dir, rot);
	}
    }
    public void translate(float mag, float dir) {
	this.drive(mag, dir, 0);
    }
    public void rotate(float rot) {
	super.mecanumDrive_Polar(0, 0, rot);
    }
    
    public void resetGyro(){
        gyro.reset();
	gyroOffset = 0;
    }
    
    public void incrementGyro(){
        gyroOffset += 2;
    }
    
    public void decrementGyro(){
        gyroOffset -= 2;
    }
    
    public boolean isUsingGyro() {
	return useGyro;
    }
    
    public void setUseGyro(boolean ug) {
	useGyro = ug;
    }
}
